//
// Created by wujehy on 18-11-14.
//

//#include <cstdio>
#include "Control.h"

void Control::init() {
    runCar=new RunCar();
    steering=new Steering();
    steering->Init();

    doCamera=0;

}

void Control::procRunCar() {
    //判断 功能是否打开
    if(doRanCar&FUNC_TAG){
        //如果功能打开 则 执行参数表的信息

        //左轮 一定 开启 则 可能向前 或者向右
        if(doRanCar&FORWARD_LEFT&&!(doRanCar&BACKWARD_LEFT)){
            //右边 也一定向前 则 一定向前 否则 只能向右
            if(doRanCar&FORWARD_RIGHT&&!(doRanCar&BACKWARD_RIGHT)){
                runCar->ForWard();
                //右前
                //车向前
//                printf("qian\n");


            } else if (!(doRanCar&(BACKWARD_RIGHT|FORWARD_RIGHT))){
                //右不前 也不后  车向右
//                printf("you\n");
                runCar->ForWardAndRightHand();
            }
         //左轮向后 则 只能是 向后 或者 后 右
        } else if (doRanCar&BACKWARD_LEFT&&!(doRanCar&FORWARD_LEFT)) {
            //右轮向后  则 向后 油轮不动则 后右
            if (doRanCar&BACKWARD_RIGHT&&!(doRanCar&FORWARD_RIGHT)){
                //
//                printf("后");
                runCar->BackWard();
            } else
                // 不动
            if(!(doRanCar&(BACKWARD_RIGHT|FORWARD_RIGHT))){
//                printf("hou 右");
                runCar->BackWardAndRightHand();
            }

        //左轮  不动 要么 向右 要么 后右 都不动 则 停止
        } else if(!(doRanCar&(FORWARD_LEFT|BACKWARD_LEFT))){
            //右向前
            if(doRanCar&FORWARD_RIGHT&&!(doRanCar&BACKWARD_RIGHT)){
                //向左
//                printf("向左");
                runCar->ForWardAndLeftHand();
            } else if(doRanCar&BACKWARD_RIGHT&&!(doRanCar&FORWARD_RIGHT)){
                //后 左
                runCar->BackWardAndLeftHand();
//                printf("后左");
            } else if (!(doRanCar&&(BACKWARD_RIGHT|FORWARD_RIGHT))){
                //ting

//                printf("stop\n");
                runCar->Stop();
            }

        }
    } else if (!(doRanCar&FUNC_TAG)){
        //stop
//        printf("stop");
        runCar->Stop();
    }
}

void Control::procCamera() {
    if (doCamera&FUNC_TAG){
        //start
        switch (doCamera&FUNC_ATTR_MASK){
            case UP:
                //shang
//                printf("up");
                steering->Up();
                break;
            case DOWN:
                //down
//                printf("dowm");
                steering->Down();
                break;

            case LEFT:
//                printf("left");
                steering->Left();

                break;
            case RIGHT:
//                printf("right");
                steering->Right();
                break;
            case STOP:
//                printf("stop");

                break;

        }

    } else if (!doCamera&FUNC_TAG){
        //stop
//        printf("stop");
    }
}

uchar Control::procLevel() {
    if (saveLevel&FUNC_TAG){
        switch (saveLevel&FUNC_ATTR_MASK){
            case LEVEL_LOW:
                runCar->LowLevel();
                steering->LowLevel();
                return LEVEL_LOW;
            case LEVEL_MIDDLE:
                runCar->MiddleLevel();
                steering->MiddleLevel();
                return  LEVEL_MIDDLE;
            case LEVEL_HIGH:
                runCar->HighLevel();
                steering->HighLevel();
                return LEVEL_HIGH;
            case LEVEL_MORE:
                runCar->MoreLevel();
                steering->MoreLevel();
                return LEVEL_MORE;
        }
    }
    //默认 低级
    runCar->LowLevel();
    steering->LowLevel();
    return LEVEL_LOW;
}

unsigned char Control::getSaveLevel() const {
    return saveLevel;
}

void Control::setMsgData(unsigned char msgData) {
    Control::msgData = msgData&0xff;
}

void Control::setDoRanCar(unsigned char doRanCar) {
    Control::doRanCar = doRanCar;
}

void Control::setDoCamera(unsigned char doCamera) {
    Control::doCamera = doCamera;
}

void Control::setSaveLevel(unsigned char saveLevel) {
    Control::saveLevel = saveLevel;
}

void Control::procMsgData() {
    //功能选择
    switch (msgData&FUNC_MASK){
        case FUNC_CAR:
            doRanCar=msgData&FUNC_MSG_MASK;
            procRunCar();
            break;
        case FUNC_CAMEAR:
            doCamera=msgData&FUNC_MSG_MASK;
            procCamera();
        case FUNC_LEVEL:
            saveLevel=msgData&FUNC_MSG_MASK;
            procLevel();
    }

}


//int main(){
//    Control control;
//    control.init();
//
//    control.setMsgData(FUNC_CAMEAR|FUNC_TAG|DOWN);
//    control.procLevel();
//    control.procMsgData();
//    return 0;
//}
